from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

def generate_launch_description():

    # 新增坐标基准参数
    base_x_arg = DeclareLaunchArgument(
        'base_x',
        default_value='0.0',
        description='Base coordinate X.'
    )

    base_y_arg = DeclareLaunchArgument(
        'base_y',
        default_value='0.0',
        description='Base coordinate Y.'
    )

    base_z_arg = DeclareLaunchArgument(
        'base_z',
        default_value='0.0',
        description='Base coordinate Z.'
    )

    # 新增坐标偏移参数
    offset_x_arg = DeclareLaunchArgument(
        'offset_x',
        default_value='0.0',
        description='Offset coordinate X.'
    )

    offset_y_arg = DeclareLaunchArgument(
        'offset_y',
        default_value='0.0',
        description='Offset coordinate Y.'
    )

    offset_z_arg = DeclareLaunchArgument(
        'offset_z',
        default_value='1.0',
        description='Offset coordinate Z.'
    )

    # 创建节点
    communication_multirotor_node = Node(
        package="drone_control",
        executable="drone_height_control",
        name="drone_height_control",
        output="screen",
        emulate_tty=True,
        parameters=[
            {"base_x": LaunchConfiguration('base_x')},  # 添加基准 X
            {"base_y": LaunchConfiguration('base_y')},  # 添加基准 Y
            {"base_z": LaunchConfiguration('base_z')},  # 添加基准 Z
            {"offset_x": LaunchConfiguration('offset_x')},  # 添加偏移 X
            {"offset_y": LaunchConfiguration('offset_y')},  # 添加偏移 Y
            {"offset_z": LaunchConfiguration('offset_z')},  # 添加偏移 Z
        ]
    )

    # 创建并返回 LaunchDescription
    return LaunchDescription([

        base_x_arg,  # 声明基准 X
        base_y_arg,  # 声明基准 Y
        base_z_arg,  # 声明基准 Z
        offset_x_arg,  # 声明偏移 X
        offset_y_arg,  # 声明偏移 Y
        offset_z_arg,  # 声明偏移 Z
        communication_multirotor_node
    ])
